/*  kinect_loopback.cpp

Copyright (C) 2010  Tisham Dhar <whatnickd ^-at-^ gmail ^-dot-^ com>

This code is licensed to you under the terms of the GNU GPL, version 2 or version 3;
see:
 http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
 http://www.gnu.org/licenses/gpl-3.0.txt
*/

#include <libfreenect.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <pthread.h>

#define CV_NO_BACKWARD_COMPATIBILITY
#include <cv.h>
#include <highgui.h>

#define FREENECTOPENCV_WINDOW_D "Depthimage"
#define FREENECTOPENCV_WINDOW_N "Normalimage"
#define FREENECTOPENCV_RGB_DEPTH 3
#define FREENECTOPENCV_DEPTH_DEPTH 1
#define FREENECTOPENCV_RGB_WIDTH 640
#define FREENECTOPENCV_RGB_HEIGHT 480
#define FREENECTOPENCV_DEPTH_WIDTH 640
#define FREENECTOPENCV_DEPTH_HEIGHT 480

pthread_mutex_t mutex_rgb = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mutex_depth = PTHREAD_MUTEX_INITIALIZER;
pthread_t vid_thread;

IplImage* depthimg = 0;
IplImage* rgbimg = 0;
IplImage* tempimg = 0;

// callback for rgbimage, called by libfreenect

void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
{


	// lock mutex for buffer rgb image
	pthread_mutex_lock( &mutex_rgb );
	memcpy(rgbimg->imageData, rgb, FREENECT_VIDEO_RGB_SIZE);
	// unlock mutex
	pthread_mutex_unlock( &mutex_rgb );
}

// callback for depthimage, called by libfreenect

void depth_cb(freenect_device *dev, void *depth, uint32_t timestamp)
{
	cv::Mat depth8;
	cv::Mat mydepth = cv::Mat( FREENECTOPENCV_DEPTH_WIDTH,FREENECTOPENCV_DEPTH_HEIGHT, CV_16UC1, depth);

	mydepth.convertTo(depth8, CV_8UC1, 1.0/4.5, -20.0);
	//
	pthread_mutex_lock( &mutex_depth );
	memcpy(depthimg->imageData, depth8.data, 640*480);
	// unlock mutex
	pthread_mutex_unlock( &mutex_depth );
}

/*
 * thread for pushing to virtual video
 */
void *vid_threadfunc (void* dev) {

	cvNamedWindow( FREENECTOPENCV_WINDOW_D, CV_WINDOW_AUTOSIZE );
	//cvNamedWindow( FREENECTOPENCV_WINDOW_N, CV_WINDOW_AUTOSIZE );

	depthimg = cvCreateImage(cvSize(FREENECTOPENCV_DEPTH_WIDTH, FREENECTOPENCV_DEPTH_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_DEPTH_DEPTH);
	tempimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);	
	rgbimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);

	// use image polling
	while (1) 
	{
		FILE *fp=fopen((char*)dev, "a+");
		setvbuf(fp,NULL,_IONBF,0);


		//lock mutex for rgb image
		pthread_mutex_lock( &mutex_rgb );
		cvCvtColor(rgbimg,tempimg,CV_BGR2RGB);
		
		/*
		fwrite(tempimg->imageData,sizeof(uint8_t),FREENECT_VIDEO_RGB_SIZE,fp);
		fflush(fp);
		*/
		cvShowImage(FREENECTOPENCV_WINDOW_D,tempimg);		
		
		//unlock mutex
		pthread_mutex_unlock( &mutex_rgb );
		
		

		//lock mutex for depth image
		pthread_mutex_lock( &mutex_depth );
		// show image to window
		cvCvtColor(depthimg,tempimg,CV_GRAY2BGR);
		cvCvtColor(tempimg,tempimg,CV_HSV2BGR);		
		
		fwrite(tempimg->imageData,sizeof(uint8_t),FREENECT_VIDEO_RGB_SIZE,fp);
		fflush(fp);
		
		//unlock mutex for depth image
		pthread_mutex_unlock( &mutex_depth );

		fclose(fp); 

		// wait for quit key
		if( cvWaitKey( 15 )==27 )
				break;
	}
	
	pthread_exit(NULL);
}

int main(int argc,char** argv)
{
	if(argc>=2)
	{
		printf("RGB Fake video device %s\n",argv[1]);
		freenect_context *f_ctx;
		freenect_device *f_dev;


		int res = 0;
		int die = 0;
		printf("Kinect camera test\n");

		if (freenect_init(&f_ctx, NULL) < 0) {
			printf("freenect_init() failed\n");
			return 1;
		}

		if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
			printf("Could not open device\n");
			return 1;
		}

		freenect_set_video_callback(f_dev, rgb_cb);
		freenect_set_depth_callback(f_dev, depth_cb);
		freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);

		// create video device dump thread
		res = pthread_create(&vid_thread, NULL, vid_threadfunc, (void*)argv[1]);
		if (res) {
			printf("pthread_create failed\n");
			return 1;
		}

		printf("init done\n");

		freenect_start_video(f_dev);
		freenect_start_depth(f_dev);

		while(!die && freenect_process_events(f_ctx) >= 0 );

	}
	else
	{
		printf("Usage: kinect_loopback <video_device>\n");
	}
}
